import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, TimerAction
from launch_ros.actions import Node


def generate_launch_description():
    # 设置 Ignition 的资源搜索路径
    pkg_path = get_package_share_directory('zorbus_description')

    urdf_path = os.path.join(pkg_path, 'urdf', 'so101_new_calib.urdf')

    controllers_path = os.path.join(pkg_path, 'config', 'ros2_controllers.yaml')
    world_path = os.path.join(pkg_path, 'worlds', 'your_world.sdf')

    return LaunchDescription([
        ExecuteProcess(
            cmd=['ign', 'gazebo', '-r', world_path],
            output='screen'
        ),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': urdf_path}]
        ),
        Node(
            package='controller_manager',
            executable='ros2_control_node',
            parameters=[
                {'robot_description': urdf_path},
                controllers_path
            ],
        ),
        TimerAction(
            period=3.0,
            actions=[
                ExecuteProcess(
                    cmd=[
                        'ros2', 'run', 'ros_gz_sim', 'create',
                        '-msg', 'robot_description',
                        '-name', 'my_robot'
                    ],
                    output='screen'
                )
            ]
        )
    ])
